#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

cv::Mat my_get_structuring_element(int shape, cv::Size shapeSize) {
    CV_Assert(shape == cv::MORPH_RECT || shape == cv::MORPH_CROSS || shape == cv::MORPH_ELLIPSE);
    
    cv::Mat mat = cv::Mat::zeros(shapeSize, CV_8UC1);
    switch (shape) {
        case cv::MORPH_RECT: {
            mat = 1;
            break;
        }
        case cv::MORPH_CROSS: {
            cv::Mat rowROI(mat, cv::Rect(0, shapeSize.height / 2, shapeSize.width, 1));
            rowROI = 1;
            cv::Mat colROI(mat, cv::Rect(shapeSize.width / 2, 0, 1, shapeSize.height));
            colROI = 1;
            break;
        }
        case cv::MORPH_ELLIPSE: {
            // (x-xc)^2 / a^2 + (y-yc)^2 / b^2 = 1
            int a = shapeSize.width / 2;
            int b = shapeSize.height / 2;
            int startx = 0, endx = 0;
            for (int i = 0; i < shapeSize.height; i++) {
                int dy = i - b;  // y - yc
                if (std::abs(dy) <= b) {  // inside/on ellipse
                    int dx = 0;
                    if (b > 0) {
                        dx = cv::saturate_cast<int>(a * std::sqrt((1.0 * b * b - 1.0 * dy * dy) / (1.0 * b * b)));  // (x - xc) / a = sqrt((b^2 - (y - yc)^2) / b^2)
                    }
                    startx = std::max(a - dx, 0);
                    endx = std::min(a + dx + 1, shapeSize.width);
                }
                uchar* p = mat.ptr<uchar>(i);
                for (int j = startx; j < endx; j++) {
                    p[j] = 1;
                }
            }
            break;
        }
        default:
            CV_Assert(false);
    }
    return mat;
}

cv::Mat my_erode_dilate(const cv::Mat& matPadded, const cv::Mat& structuringElement, bool isErode) {
    CV_Assert(matPadded.type() == CV_8UC1);
    CV_Assert(structuringElement.type() == CV_8UC1);
    
    int rows = matPadded.rows - structuringElement.rows + 1;
    int cols = matPadded.cols - structuringElement.cols + 1;
    cv::Mat mat(rows, cols, CV_8UC1);
    for (int i = 0; i < rows; i++) {
        uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < cols; j++) {
            cv::Mat roi(matPadded, cv::Rect(j, i, structuringElement.cols, structuringElement.rows));
            std::vector<uchar> v;
            for (int ii = 0; ii < roi.rows; ii++) {
                uchar* pp = roi.ptr<uchar>(ii);
                const uchar* ppp = structuringElement.ptr<uchar>(ii);
                for (int jj = 0; jj < roi.cols; jj++) {
                    if (ppp[jj] == 1) {
                        v.push_back(pp[jj]);
                    }
                }
            }
            std::sort(v.begin(), v.end());
            p[j] = (isErode ? v[0] : v[v.size()-1]);
        }
    }
    return mat;
}

int count_non_zeros(const cv::Mat& mat) {
    CV_Assert(mat.type() == CV_8UC1);
    int cnt = 0;
    for (int i = 0; i < mat.rows; i++) {
        const uchar* p = mat.ptr<uchar>(i);
        for (int j = 0; j < mat.cols; j++) {
            if (p[j] != 0) {
                cnt++;
            }
        }
    }
    return cnt;
}
                    

int main(int argc, char **argv) {
    /*int shape = cv::MORPH_RECT;
    cv::Size shapeSize(7, 8);
    cv::Mat rectStructuringElement = cv::getStructuringElement(shape, shapeSize);
    cv::Mat myRectStructuringElement = my_get_structuring_element(shape, shapeSize);
    std::cout << "rectStructuringElement = \n" << rectStructuringElement << std::endl << std::endl;
    std::cout << "myRectStructuringElement = \n" << myRectStructuringElement << std::endl << std::endl;
    
    shape = cv::MORPH_CROSS;
    cv::Mat crossStructuringElement = cv::getStructuringElement(shape, shapeSize);
    cv::Mat myCrossStructuringElement = my_get_structuring_element(shape, shapeSize);
    std::cout << "crossStructuringElement = \n" << crossStructuringElement << std::endl << std::endl;
    std::cout << "myCrossStructuringElement = \n" << myCrossStructuringElement << std::endl << std::endl;
    
    shape = cv::MORPH_ELLIPSE;
    cv::Mat ellipseStructuringElement = cv::getStructuringElement(shape, shapeSize);
    cv::Mat myEllipseStructuringElement = my_get_structuring_element(shape, shapeSize);
    std::cout << "ellipseStructuringElement = \n" << ellipseStructuringElement << std::endl << std::endl;
    std::cout << "myEllipseStructuringElement = \n" << myEllipseStructuringElement << std::endl << std::endl;
    */
    
    cv::Mat img = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    if (img.empty()) {
        std::cout << "Failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    int shape = cv::MORPH_ELLIPSE;
    cv::Size shapeSize(7, 8);
    cv::Mat structuringElement = cv::getStructuringElement(shape, shapeSize, cv::Point(-1, -1));
    cv::Mat matErode, matDilate;
    cv::erode(img, matErode, structuringElement, cv::Point(-1, -1), 1, cv::BORDER_REFLECT101);
    cv::dilate(img, matDilate, structuringElement, cv::Point(-1, -1), 1, cv::BORDER_REFLECT101);
    
    cv::Mat matPadded;
    int left = structuringElement.cols / 2;
    int right = structuringElement.cols % 2 == 0 ? structuringElement.cols / 2 - 1 : structuringElement.cols / 2;
    int top = structuringElement.rows / 2;
    int bottom = structuringElement.rows % 2 == 0 ? structuringElement.rows / 2 - 1 : structuringElement.rows / 2;
    cv::copyMakeBorder(img, matPadded, top, bottom, left, right, cv::BORDER_REFLECT101);
    cv::Mat myMatErode, myMatDilate;
    myMatErode = my_erode_dilate(matPadded, structuringElement, true);
    myMatDilate = my_erode_dilate(matPadded, structuringElement, false);
    
    cv::Mat matErodeDiff, matDilateDiff;
    matErodeDiff = myMatErode - matErode;
    matDilateDiff = myMatDilate - matDilate;
    std::vector<cv::Point> nonZerosErode, nonZerosDilate;
    cv::findNonZero(matErodeDiff, nonZerosErode);
    cv::findNonZero(matDilateDiff, nonZerosDilate);
    std::cout << "nonZerosErode = " << nonZerosErode << std::endl;
    std::cout << "nonZerosDilate = " << nonZerosDilate << std::endl;
    std::cout << "non zeros count erode = " << count_non_zeros(matErodeDiff) << std::endl;
    std::cout << "non zeros count dilate = " << count_non_zeros(matDilateDiff) << std::endl;
    
    cv::imshow("source", img);
    cv::imshow("erode", matErode);
    cv::imshow("dilate", matDilate);
    cv::imshow("myerode", myMatErode);
    cv::imshow("mydilate", myMatDilate);
    cv::waitKey(0);
    
    return EXIT_SUCCESS;
}
